#include "main.h"

void init(void)
{
	/* Variables */
	usartDataEnqueuePosition = 0;
	usartDataDequeuePosition = 0;
	usartDataDequeuePositionRoundCounter = 0;
	usartDataEnqueuePositionRoundCounter = 0;
	
	USART	= USART_STATE_IDLE;
	TWI		= TWI_STATE_IDLE;
	
	usartByteCounter	= 0;
	timerCounter		= 0;
	
	// Input/output
	DDRB = 0x0;			// wait mode
	DDRA = 0xFF;		// lights
	
	// communication
	initUSART();
	initTWIMaster();

	/* Presacler  1024 */ 
	TCCR0|=(1<<CS02)|(1<<CS00);
	
	/* Overflow Interrupt
	*   Set counter 0
	**/
	TIMSK|=(1<<TOIE0);
	TCNT0=0;
	
	D_LIGHT_ON(PINA0)
	D_LIGHT_ON(PINA1)
	D_LIGHT_ON(PINA2)
	D_LIGHT_ON(PINA3)
	
	// Wait for start condtion
	while(!WAIT_MODE);
	
	D_LIGHT_OFF(PINA0)
	D_LIGHT_OFF(PINA1)
	D_LIGHT_OFF(PINA2)
	D_LIGHT_OFF(PINA3)
	
	sei();
}

ISR(TIMER0_OVF_vect)
 {
	if(++timerCounter % 32 == 0)
	{	
		TWI = TWI_STATE_GSEN;
	}
 }

ISR(USART_RXC_vect)
{
	// Get data
	int8_t data = receiveUSART();
	
	/* Check if connected data 
	* If USART state is not incoming and the data sends 
	* a start condition then act. 
	**/ 
	
	if((USART == USART_STATE_IDLE) && (data == USART_DATA_START))
	{
		USART = USART_STATE_INCOMING;
		usartByteCounter = 0;
	}	
	else if((USART == USART_STATE_INCOMING) && usartByteCounter > 4 && (data == USART_DATA_STOP))
	{
		USART = USART_STATE_MANAGE;
	}
	
	// Incoming connected bytes? count the bytes
	if(USART == USART_STATE_INCOMING)
		usartByteCounter++;
		
	// Insert data to buffer
	enqueue(data);
}

/* FIFO - Insert value*/
void enqueue(int8_t data)
{
	usartData[usartDataEnqueuePosition++] = data;
	
	if(usartDataEnqueuePosition == 0)
		usartDataEnqueuePositionRoundCounter++;	
}

/* FIFO - Get value */
uint8_t dequeue()
{
	if (peek() == FALSE)
		return -1;
			
	uint8_t data =  usartData[usartDataDequeuePosition++];
	
	if(usartDataDequeuePosition == 0)
		usartDataDequeuePositionRoundCounter++;
		
	return data;
}

/* FIFO - Returns TRUE if there is data to read */
uint8_t peek()
{
	if (usartDataDequeuePosition >= usartDataEnqueuePosition && 
	(usartDataEnqueuePositionRoundCounter == usartDataDequeuePositionRoundCounter))
		return FALSE;
	return TRUE;
}

/* TWI - Send */
void sendtoTWI(const int8_t address, const int8_t data)
{
START_S:
	if(twiStart() == -1 || twiCallSlaveToWrite(address) == -1 || twiWrite(data) == -1)
	{
		twiStop();
		goto START_S;
	}
	twiStop();
}

/* TWI - Recivce */
int8_t recieveTWI(const int8_t address)
{
	volatile uint8_t data = 0;
	START_R:
	if(twiStart() == -1 || twiCallSlaveToRead(address) == -1 )
	{
		twiStop();
		goto START_R;
	}
	
	data = twiRead();

	if(data == -1)
	{
		twiStop();
		goto START_R;
	}
	
	twiStop();
	return data;
}

int main(void)
{
	init();
    while(1)
	
    {
		switch(TWI)
		{
			case TWI_STATE_GSEN: // Get sensor values
			{
				transmitUSART(TWI_DATA_START);
				transmitUSART(recieveTWI(SENENH_SLA_R));
				transmitUSART(recieveTWI(SENENH_SLA_R));
				transmitUSART(recieveTWI(SENENH_SLA_R));
				transmitUSART(recieveTWI(SENENH_SLA_R));
				transmitUSART(recieveTWI(SENENH_SLA_R));
				
				transmitUSART(recieveTWI(SENENH_SLA_R));	// LineSensor
				
				transmitUSART(TWI_DATA_STOP);				

				// Reset twi
				TWI = TWI_STATE_IDLE;
				break;
			}
			case TWI_STATE_GREG: // Get regualtion values
			{
				
				break;				
			}
			case TWI_STATE_IDLE:
			default:
			{
				break;
			}
							
		}
		
		switch(USART)
		{
			case USART_STATE_IDLE:
			{
				// Manage control signal
				if(peek() == TRUE && USART == USART_STATE_IDLE)
				{
					sendtoTWI(STYENH_SLA_W,dequeue());
				}					
				break;		
			}
			case USART_STATE_MANAGE:
			{
				// Manage start + 4 bytes + stop
				
				// start condition
				dequeue();
				uint8_t data = dequeue();
				if(data == USART_DATA_CALIBRATION)
				{
					D_LIGHT_ON(PINA0)
					// Sensor
					sendtoTWI(SENENH_SLA_W,USART_DATA_START);
					_delay_us(300);
					sendtoTWI(SENENH_SLA_W,data);
					int i;
					for(i=0; i<4;i++)
					{
						_delay_us(300);
						sendtoTWI(SENENH_SLA_W,dequeue());
					}
					D_LIGHT_OFF(PINA0)
				}
				else if(data == USART_DATA_CALIBRATE)
				{
					D_LIGHT_ON(PINA0)
					// Sensor
					sendtoTWI(SENENH_SLA_W,USART_DATA_START);
					_delay_us(300);
					sendtoTWI(SENENH_SLA_W,data);
					int i;
					for(i=0; i<4;i++)
					{
						_delay_us(300);
						sendtoTWI(SENENH_SLA_W,dequeue());
					}
					D_LIGHT_OFF(PINA0)
				}
				else
				{
					D_LIGHT_ON(PINA3)
					// styr
					sendtoTWI(STYENH_SLA_W,USART_DATA_START);
					_delay_us(300);
					sendtoTWI(STYENH_SLA_W,data);
					int i;
					for(i=0; i<4;i++)
					{
						_delay_us(300);
						sendtoTWI(STYENH_SLA_W,dequeue());
					}
					D_LIGHT_OFF(PINA3)
				}
							
				// Mange done
				USART = USART_STATE_IDLE;
				break;	
			}
			case USART_STATE_INCOMING:
			default:
			{
				break;				
			}	
		}
	}
}